import {
  Cartesian3,
  HeadingPitchRoll,
  Ellipsoid,
  Matrix4,
  Transforms,
  Matrix3,
  Quaternion,
} from "cesium";

export function getModelMatrix(pointA: Cartesian3, pointB: Cartesian3) {
  //向量AB
  const vector2 = Cartesian3.subtract(pointB, pointA, new Cartesian3());
  //归一化
  const normal = Cartesian3.normalize(vector2, new Cartesian3());
  //旋转矩阵 rotationMatrixFromPositionVelocity源码中有，并未出现在cesiumAPI中
  const rotationMatrix3 = Transforms.rotationMatrixFromPositionVelocity(
    pointA,
    normal,
    Ellipsoid.WGS84
  );
  const modelMatrix4 = Matrix4.fromRotationTranslation(rotationMatrix3, pointA);
  return getHeadingPitchRoll(modelMatrix4);
}

function getHeadingPitchRoll(m: Matrix4) {
  var m1 = Transforms.eastNorthUpToFixedFrame(
    Matrix4.getTranslation(m, new Cartesian3()),
    Ellipsoid.WGS84,
    new Matrix4()
  );
  // 矩阵相除
  var m3 = Matrix4.multiply(
    Matrix4.inverse(m1, new Matrix4()),
    m,
    new Matrix4()
  );
  // 得到旋转矩阵
  var mat3 = Matrix4.getMatrix3(m3, new Matrix3());
  // 计算四元数
  var q = Quaternion.fromRotationMatrix(mat3);
  // 计算旋转角(弧度)
  var hpr = HeadingPitchRoll.fromQuaternion(q);
  return hpr;
}
